import enum

# from typing import override
import math
import numpy as np
import json

from sls_controller.sls_missionsABC import Missions, MissionsState
from sls_controller.sls_publicDefine import ParamDefault
import math
from geopy.distance import geodesic
from geopy.distance import geodesic
from std_msgs.msg import Float64MultiArray
from math import atan2, radians, degrees, sin, cos, pi, tan, hypot,acos
from geometry_msgs.msg import Twist
from nav_msgs.msg import Odometry
from scipy.spatial.transform import Rotation as R
from sls_msgs.msg import LlaVelocity
from sls_mirrors.sls_agent import Mirror
from sls_mirrors.sls_UAVmirrorsUpdate import BasicPositionMirrorUpdate


class ShiftLevel(enum.Enum):
    D = 1
    N = 2
    R = 3


class DriveMode(enum.Enum):
    Non = 0
    Auto = 1
    Pad = 2
    Remote = 3


def lla2ecef(lon, lat, alt):
    """
    unit of inputs should be rad
    """
    a = 6378137.0  # a axis
    f = 1 / 298.257223565  # f = (a-b)/a
    e_square = f * (2 - f)
    N = a / math.sqrt(1 - e_square * math.sin(lat) ** 2)
    X = (N + alt) * math.cos(lat) * math.cos(lon)
    Y = (N + alt) * math.cos(lat) * math.sin(lon)
    Z = (N * (1 - e_square) + alt) * math.sin(lat)

    return [X, Y, Z]


def lla2enu(lon, lat, alt, lon0, lat0, alt0):
    p0 = lla2ecef(lon0, lat0, alt0)
    p = lla2ecef(lon, lat, alt)
    [u, v, w] = [p[0] - p0[0], p[1] - p0[1], p[2] - p0[2]]
    cos_lon0 = math.cos(lon0)
    sin_lon0 = math.sin(lon0)
    cos_lat0 = math.cos(lat0)
    sin_lat0 = math.sin(lat0)

    e = -sin_lon0 * u + cos_lon0 * v
    t = cos_lon0 * u + sin_lon0 * v

    n = -sin_lat0 * t + cos_lat0 * w
    u = cos_lat0 * t + sin_lat0 * w
    # if e << 1, there is the approximation:
    # e = a * math.cos(lat)*delta_lon
    # n = a * delta_lat
    # u = delta_alt
    return [e, n, u]


def deg2rad(x):
    return x * math.pi / 180.0


class StanelyController:
    def __init__(self, param: dict):
        self.k1 = param.get("k1", 0.2)
        self.k2 = param.get("k2", 0.1)

    def init(self):
        pass

    def step(self, *, lat_err: float, yaw_err: float) -> float:
        return -(self.k1 * lat_err + self.k2 * yaw_err)


class MissionsPathTracking(Missions):
    def __init__(
        self,
        publicResource,
        datasets: dict = None,
    ):
        super().__init__(publicResource, datasets)
        # pulicResource and dataset have been added in Missions constructor
        self.fore_counter = round(
            1.0 / ParamDefault.MAIN_TIMER_DURING
        )  # waiting 1s before start

        if self.datasets is None or "path_data" not in self.datasets:
            print(f"need path data")
            raise RuntimeError

        self.origin = None
        self.enu = None
        self.path_data = np.array(self.datasets["path_data"])
        self.controller = StanelyController(self.datasets["controller_param"])
        self.lla_list = []

        # datasets like this: d=  {"path_data": [[]], "controller_param": {"k1": 0.2, "k2": 0.1}}

    def fore_check(self):
        lla = self.publicResource.ugv_resource.get_gps_position()
        if lla is None:
            return False
        (lon, lat, alt) = lla
        self.lla_list.append([lon, lat, alt])
        if len(self.lla_list) >= 101:
            self.lla_list.pop(0)
        if self.origin is None:
            if len(self.lla_list) >= 100:
                avg_lon = sum(d[0] for d in self.lla_list) / len(self.lla_list)
                avg_lat = sum(d[1] for d in self.lla_list) / len(self.lla_list)
                avg_alt = sum(d[2] for d in self.lla_list) / len(self.lla_list)
                self.origin = [avg_lon, avg_lat, avg_alt]

        if self.origin is None:
            return False

        print(f"the origin point is set to {self.origin=}")
        return True

    def done_check(self):
        # TODO:
        print(f"{self.publicResource.ugv_resource.get_gps_position()=}")
        return False

    def mission_action(self):
        (lon, lat, alt) = self.publicResource.ugv_resource.get_gps_position()
        self.enu = lla2enu(
            deg2rad(lon),
            deg2rad(lat),
            alt,
            deg2rad(self.origin[0]),
            deg2rad(self.origin[1]),
            self.origin[2],
        )
        yaw = self.publicResource.ugv_resource.get_gps_yaw()
        yaw = deg2rad(((360.0 - yaw) + 90) % 360.0)

        x = self.enu[0]
        y = self.enu[1]
        z = self.enu[2]

        s = self.path_data[0, :]
        xd = self.path_data[1, :]
        yd = self.path_data[2, :]
        theta_d = self.path_data[3, :]

        idx = np.argmin((x - xd) ** 2 + (y - yd) ** 2)

        lat_err = -(x - xd[idx]) * np.sin(theta_d[idx]) + (y - yd[idx]) * np.cos(
            theta_d[idx]
        )
        yaw_err = yaw - theta_d[idx]
        if yaw_err > math.pi:
            yaw_err -= 2 * math.pi
        elif yaw_err < -math.pi:
            yaw_err += 2 * math.pi

        if not (-math.pi <= yaw_err <= math.pi):
            self.get_logger().warning("yaw_err calc error")

        self.publicResource.ugv_resource.platform_interface.main_node.get_logger().info(
            f"origin: {self.origin} local position: {self.enu=} error: {lat_err=} {yaw_err=}"
        )

        steer = self.controller.step(lat_err=lat_err, yaw_err=yaw_err)
        speed = 1.0  # m/s
        self.publicResource.ugv_resource.platform_interface.main_node.get_logger().info(
            f"control input: {steer=} {speed=}"
        )
        self.publicResource.ugv_resource.pub_vehicle_cmd(
            mode=DriveMode.Auto,
            shift=ShiftLevel.D,
            speed=speed,
            steer_angle=steer,
            brake=0,
        )

    def mission_enter(self):
        self.controller.init()
        print("mission enter")

    def mission_exit(self):
        print("mission exit")

    def abort_check(self):
        pass

    def mission_abort(self):
        pass


class MissionsSpin(Missions):
    def __init__(
        self,
        publicResource,
        datasets: dict = None,
    ):
        super().__init__(publicResource, datasets)

    def fore_check(self):
        return True

    def done_check(self):
        # spin fever
        return False

    def mission_action(self):
        pass

    def mission_enter(self):
        pass

    def mission_exit(self):
        pass

    def abort_check(self):
        pass

    def mission_abort(self):
        pass

class StanelyController:
    def __init__(self, param: dict):
        self.k1 = param.get("k1", 0.2)
        self.k2 = param.get("k2", 0.1)

    def init(self):
        pass

    def step(self, *, lat_err: float, yaw_err: float) -> float:
        return -(self.k1 * lat_err + self.k2 * yaw_err)

from sensor_msgs.msg import LaserScan

class MissionsSimple(Missions):
    def __init__(
        self,
        publicResource,
        datasets: dict = None,
    ):
        self.publicResource = publicResource
        super().__init__(publicResource, datasets)
        self.publicResource.ros_resource.create_subscription(LaserScan,"scan",self.laserCallback,10)

        self.rplidar_sec = None
        self.rplidar_nanosec = None
        self.rplida_fram_id = None
        self.angle_min = None
        self.angle_max = None
        self.angle_increment = None
        self.time_increment = None
        self.scan_time = None
        self.range_min = None
        self.range_max = None
        self.ranges = None
        self.intensities = None

        self.rplidar_front = None

    def fore_check(self):
        return True

    def done_check(self):
        return False

    def mission_action(self):
        self.publicResource.ugv_resource.set_simu_vehicle_cmd(drive_mode_reg =1, shift_level_reg = 1,
                                                              steering_wheel_angle=120,
                                                              target_speed=3.6,brake_mode=0)

    def mission_enter(self):
        print("mission enter")

    def mission_exit(self):
        print("mission exit")

    def abort_check(self):
        pass

    def mission_abort(self):
        pass

    def laserCallback(self,msg:LaserScan):
        self.rplidar_sec = msg.header.stamp.sec
        self.rplidar_nanosec = msg.header.stamp.nanosec
        self.rplida_fram_id = msg.header.frame_id
        self.angle_min = msg.angle_min
        self.angle_max = msg.angle_max
        self.angle_increment = msg.angle_increment
        self.time_increment = msg.time_increment
        self.scan_time = msg.scan_time
        self.range_min = msg.range_min
        self.range_max = msg.range_max
        self.ranges = msg.ranges
        self.intensities = msg.intensities

        self.rplidar_front = self.ranges[810:2430]


from geopy.distance import geodesic
from geographiclib.geodesic import Geodesic

class MissionsCarFollowPoint(Missions):
    def __init__(self,publicResource,datasets:dict={
                                                        "GNSS_position": [
                                                            32.1994253,
                                                            119.5074,
                                                            0.0
                                                        ],
                                                        "vehicle_yaw": 0.
                                                    }):
        super().__init__(publicResource,datasets)
        self.dest_lon = datasets["GNSS_position"][1]
        self.dest_lat = datasets["GNSS_position"][0]
        
    def fore_check(self):
        return True

    def done_check(self):
        if (self.publicResource.ugv_resource.get_gps_position() is not None):
            print((self.publicResource.ugv_resource.get_gps_position()["latitude"],
                        self.publicResource.ugv_resource.get_gps_position()["longitude"]))
            if self.calculate_gps_distance((self.publicResource.ugv_resource.get_gps_position()["latitude"],
                        self.publicResource.ugv_resource.get_gps_position()["longitude"])
                        ,(self.dest_lat,self.dest_lon)
                        ) <3.:
                cmd_msg = Float64MultiArray()
                cmd_msg.data = [1, 1, 0, 0, 1]
                self.publicResource.ugv_resource.set_simu_vehicle_cmd(target_speed=0.0,steering_wheel_angle=0.0,brake_mode = 1,shift_level_reg = 1 ,drive_mode_reg=1)
                # self.publicResource.ugv_resource.set_vehicle_several_cmd(steering_wheel_angle_deg=0.,brk_en=1)
                # self.publicResource.ugv_resource.set_vehicle_several_cmd(steering_wheel_angle_deg=0.,brk_en=1)
                return True
        return False

    def mission_action(self):
        if self.publicResource.ugv_resource.get_gps_position()is not None:
            print(self.publicResource.ugv_resource.get_gps_position())
            yaw = self.dyaw((self.publicResource.ugv_resource.get_gps_position()["latitude"],
                            self.publicResource.ugv_resource.get_gps_position()["longitude"])
                            ,(self.dest_lat,self.dest_lon)
                            )
            enu_yaw = (450 - self.publicResource.ugv_resource.get_gps_yaw())%360

            x, y = self.gps_to_local(self.publicResource.ugv_resource.get_gps_position()["latitude"], 
                                     self.publicResource.ugv_resource.get_gps_position()["longitude"],
                                 self.dest_lat, self.dest_lon)
            target_heading = atan2(y, x)
            heading_error = target_heading - self.publicResource.ugv_resource.get_gps_yaw()/57.3
            heading_error = (heading_error + math.pi) % (2 * math.pi) - math.pi  
            steer_vel = 5 * heading_error

            self.publicResource.ugv_resource.set_simu_vehicle_cmd(target_speed=1.0,steering_wheel_angle=steer_vel,brake_mode = 0,shift_level_reg = 1,drive_mode_reg=1)
            # self.publicResource.ugv_resource.set_vehicle_several_cmd(speed_mps=0.5,steering_wheel_angle_deg=(enu_yaw-yaw*57.3))
            # self.publicResource.ugv_resource.set_vehicle_several_cmd(speed_mps=0.5,steering_wheel_angle_deg=(enu_yaw-yaw*57.3))
    
    def mission_enter(self):
        pass
    def mission_exit(self):
        pass


    def abort_check(self):
        pass
    
    def mission_abort(self):
        pass

    def gps_to_local(self, lat1, lon1, lat2, lon2):
        geod = Geodesic.WGS84
        g = geod.Inverse(lat1, lon1, lat2, lon2)
        azimuth = g['azi1']  # 从起点到终点的方位角（度）
        distance = g['s12']  # 距离（米）

        # 将极坐标 (distance, azimuth) 转换为笛卡尔坐标 (东, 北)
        east = distance * math.sin(math.radians(azimuth))
        north = distance * math.cos(math.radians(azimuth))
        return east, north
    
    def dyaw(self, GPS1: tuple, GPS2: tuple) -> float:
        """
        计算GPS1向GPS2的夹角在ENU坐标系下的yaw轴角度
        返回：从正北方向开始的角度（弧度），逆时针为正
        """
        lat1 = math.radians(GPS1[0])
        lon1 = math.radians(GPS1[1])
        lat2 = math.radians(GPS2[0])
        lon2 = math.radians(GPS2[1])
        dlon = lon2 - lon1
        yaw = math.atan2(
            math.sin(dlon) * math.cos(lat2),
            math.cos(lat1) * math.sin(lat2) - 
            math.sin(lat1) * math.cos(lat2) * math.cos(dlon)
        )
        # 规范化到[-π, π]
        yaw = math.atan2(math.sin(yaw), math.cos(yaw))
        return yaw
    
    def calculate_gps_distance(self, gps1: tuple, gps2: tuple) -> float:
        """
        计算两个GPS坐标点之间的球面距离（大圆距离）
        
        参数:
        gps1: 第一个GPS坐标 (lat1, lon1) 单位：度
        gps2: 第二个GPS坐标 (lat2, lon2) 单位：度
        
        返回:
        distance: 两点之间的距离，单位：米
        """
            
        # 将角度转换为弧度
        lat1 = math.radians(gps1[0])
        lon1 = math.radians(gps1[1])
        lat2 = math.radians(gps2[0])
        lon2 = math.radians(gps2[1])
        
        # 地球半径（米）
        R = 6371000  # 平均地球半径
        
        # 使用Haversine公式计算球面距离
        dlat = lat2 - lat1
        dlon = lon2 - lon1
        
        a = (math.sin(dlat/2) * math.sin(dlat/2) + 
            math.cos(lat1) * math.cos(lat2) * 
            math.sin(dlon/2) * math.sin(dlon/2))
        
        c = 2 * math.atan2(math.sqrt(a), math.sqrt(1-a))
        
        distance = R * c
        
        return distance

class MissionsCarFollowDrone(Missions):
    def __init__(self,publicResource,datasets:dict={
                                                        "GNSS_position": [
                                                            32.1994253,
                                                            119.5074,
                                                            0.0
                                                        ],
                                                        "vehicle_yaw": 0.
                                                    }):
        super().__init__(publicResource,datasets)
        self.dest_lon = 119.5074
        self.dest_lat = 32.1994253
        
    def fore_check(self):
        return True

    def done_check(self):
        if (self.publicResource.ugv_resource.get_gps_position() is not None):
            if self.publicResource.tools_resource.calculate_gps_distance((self.publicResource.ugv_resource.get_gps_position()["latitude"],
                        self.publicResource.ugv_resource.get_gps_position()["longitude"])
                        ,(self.dest_lat,self.dest_lon)
                        ) <2:
                # self.publicResource.ugv_resource.set_simu_vehicle_cmd(target_speed=0.0,steering_wheel_angle=0.0,brake_mode = 1,shift_level_reg = 1 ,drive_mode_reg=1)
                self.publicResource.ugv_resource.set_vehicle_several_cmd(steering_wheel_angle_deg=0.,brk_en=1)
                # self.publicResource.ugv_resource.set_vehicle_several_cmd(steering_wheel_angle_deg=0.,brk_en=1)
                return True
        return False

    def mission_action(self):
        if self.publicResource.ugv_resource.get_gps_position()is not None:
            (x, y, yaw) = self.publicResource.tools_resource.gps_to_local_NED(
                (self.publicResource.ugv_resource.get_gps_position()["latitude"], 
                                     self.publicResource.ugv_resource.get_gps_position()["longitude"]),
                                 (self.dest_lat, self.dest_lon))
            target_heading = atan2(y, x)*57.3
            target_heading = self.publicResource.tools_resource.swap_NED_ENU_yaw(target_heading)
            target_heading = self.publicResource.tools_resource.normalize_angle_180(target_heading)
            heading_error_deg = target_heading - self.publicResource.ugv_resource.get_gps_yaw()
            # print(f"{self.publicResource.ugv_resource.get_gps_yaw()=}")
            # print(f"{self.publicResource.tools_resource.normalize_angle_180(self.publicResource.ugv_resource.get_gps_yaw())=}")

            print(target_heading)
            heading_error_deg = self.publicResource.tools_resource.normalize_angle_180(heading_error_deg)
            steer_vel = 5*heading_error_deg/57.3
            # print(self.publicResource.ugv_resource.get_gps_position())
            # self.publicResource.ugv_resource.set_simu_vehicle_cmd(target_speed=1.0,steering_wheel_angle=steer_vel,brake_mode = 0,shift_level_reg = 1,drive_mode_reg=1)
            # print(heading_error_deg)
            # self.publicResource.ugv_resource.set_simu_vehicle_cmd(target_speed=1.0,steering_wheel_angle=steer_vel,brake_mode = 0,shift_level_reg = 1,drive_mode_reg=1)
            self.publicResource.ugv_resource.set_vehicle_several_cmd(speed_mps=0.5,steering_wheel_angle_deg=heading_error_deg,brk_en=0)
            # self.publicResource.ugv_resource.set_vehicle_several_cmd(speed_mps=0.5,steering_wheel_angle_deg=(enu_yaw-yaw*57.3))
    
    def mission_enter(self):
                #step to use the mirror
        #1.create ugv mirror
        uav_mirror = Mirror("uav1")
        #2. add mirror updates to mirror which must before the mirror is added to agent
        uav_mirror.add_mirror_updates(BasicPositionMirrorUpdate)
        #3. add mirror to agent
        self.publicResource.mirrors_agent.add_mirrors(uav_mirror)
        #4. add notify func to mirror update
        self.publicResource.mirrors_agent.add_notify_func_to_mirror_update("uav1",BasicPositionMirrorUpdate,self.uav_position_update)
    def mission_exit(self):
        pass


    def abort_check(self):
        pass
    
    def mission_abort(self):
        pass

    def uav_position_update(self,mirror,mirrorUpdate:BasicPositionMirrorUpdate):
        self.uav_position = mirrorUpdate.GNSS_position
        if self.uav_position == None:
            self.uav_latitude = -1.0
            self.uav_longitude = -1.0
            self.uav_iseffect = False
        else:
            self.uav_longitude = self.uav_position["longitude"]*1.0
            self.uav_latitude = self.uav_position["latitude"]*1.0
            self.uav_iseffect = True
            self.dest_lon =  self.uav_longitude
            self.dest_lat = self.uav_latitude
        # print(f"{self.uav_latitude = },{self.uav_longitude = }")
    
from lifecycle_msgs.srv import ChangeState
from lifecycle_msgs.msg import Transition
import time,rclpy
class MissionsLifecycleActivate(Missions):
    def __init__(self, publicResource, datasets: dict = None):
        super().__init__(publicResource, datasets)

    def fore_check(self):
        return True

    def done_check(self):
        self.publicResource.ros_resource.destroy_client(self.client)
        return True

    def mission_action(self):
        pass
    
    def mission_enter(self):
        self.client = self.publicResource.ros_resource.create_client(ChangeState, '/lc_talker/change_state')
        while not self.client.wait_for_service(timeout_sec=1.0):
            pass
        configure_request = ChangeState.Request()
        configure_request.transition.id = Transition.TRANSITION_CONFIGURE
        configure_future = self.client.call_async(configure_request)
        rclpy.spin_until_future_complete(self.publicResource.ros_resource.main_node, configure_future)
        
        if configure_future.result() is not None and configure_future.result().success:
            activate_request = ChangeState.Request()
            activate_request.transition.id = Transition.TRANSITION_ACTIVATE
            activate_future = self.client.call_async(activate_request)
            rclpy.spin_until_future_complete(self.publicResource.ros_resource.main_node, activate_future)
            
            if activate_future.result() is not None and activate_future.result().success:
                return True
            else:
                return False
        else:
            return False

    def mission_exit(self):
        pass

    def abort_check(self):
        pass
    
    def mission_abort(self):
        pass


class MissionsLifecycleDeactivate(Missions):
    def __init__(self, publicResource, datasets: dict = None):
        super().__init__(publicResource, datasets)

    def fore_check(self):
        return True

    def done_check(self):
        self.publicResource.ros_resource.destroy_client(ChangeState,'/lc_talker/change_state')
        return True

    def mission_action(self):
        pass
    
    def mission_enter(self):
        self.client = self.publicResource.ros_resource.create_client(ChangeState, '/lc_talker/change_state')
        while not self.client.wait_for_service(timeout_sec=1.0):
            pass
        request = ChangeState.Request()
        request.transition.id = Transition.TRANSITION_DEACTIVATE

        future = self.client.call_async(request)
        rclpy.spin_until_future_complete(self.publicResource.ros_resource.main_node, future)
        if future.result() is not None and future.result().success:
            print('ok')
        pass

    def mission_exit(self):
        pass

    def abort_check(self):
        pass
    
    def mission_abort(self):
        pass

from sls_controller.sls_libs import pub_stm32
import polyline
class MissionsSTM32Serialpub(Missions):
    def __init__(self, publicResource, datasets: dict = None):
        super().__init__(publicResource, datasets)
        self.lat_lon = polyline.decode(self.datasets["lat_lon_decoded"],precision=8)
        self.stm32 = pub_stm32.STM32Communicator("/dev/ttyUSB1")
        # self.enu = []
    def fore_check(self):
        return True

    def done_check(self):
        if len(self.lat_lon) < 31:
            self.stm32.set_running_status(4)  # 停止
            self.stm32.close()
            return True
        return False

    def mission_action(self):
        min_distance = float('inf')
        min_index = -1
        cur_lat = self.publicResource.ugv_resource.get_gps_position()["latitude"]
        cur_lon = self.publicResource.ugv_resource.get_gps_position()["longitude"]
        for index, point in enumerate(self.lat_lon[0:30]):
            dis = self.publicResource.tools_resource.calculate_gps_distance(
                (cur_lat,cur_lon),(point[0],point[1])
            )
            # print(point[0],point[1])
            # print((cur_lat,cur_lon))
            # print(dis)
            if dis < min_distance:
                min_distance = dis
                min_index = index
        if min_distance < 0.5:
            self.lon_lat = self.lon_lat[min_index:]
        # 路径 yaw
        path_yaw = self.publicResource.tools_resource.calculate_heading_NED((self.lon_lat[0][1],self.lon_lat[0][0]),
                                                (self.lon_lat[1][1],self.lon_lat[1][0]))

        # NED->ENU
        path_yaw = self.publicResource.tools_resource.swap_NED_ENU_yaw(path_yaw*57.3)
        # any -> (-180-180)
        path_yaw = self.publicResource.tools_resource.normalize_angle_180(path_yaw)

        # GPS yaw
        # NED->ENU
        rtk_yaw = self.publicResource.tools_resource.swap_NED_ENU_yaw(self.publicResource.ugv_resource.get_gps_yaw())
        # any -> (-180-180)
        rtk_yaw = self.publicResource.tools_resource.normalize_angle_180(rtk_yaw)

        lat_err = self.publicResource.tools_resource.calculate_cross_track((cur_lon,cur_lat),
                                                                           ((point[0],point[1])))
        yaw_err = self.publicResource.tools_resource.normalize_angle_180(rtk_yaw - path_yaw)
        # print(f"{lat_err=},{yaw_err=}")
        self.stm32.send_control_data(lat_err, yaw_err/57.3)

    def mission_enter(self):
        self.stm32.set_running_status(1)
        # for i in self.datasets["lon_lan"]:
        #     self.enu.append(self.ll2en(i))
        pass

    def mission_exit(self):
        pass

    def abort_check(self):
        pass
    
    def mission_abort(self):
        pass

class MissionsMultitudePoints(Missions):
    BEFORE_NUM = 100
    AFTER_NUM = 300
    def __init__(self, publicResource, datasets: dict = None):
        super().__init__(publicResource, datasets)
        params = {
            "k1": 0.55,
            "k2": 0.40
        }
        self.controller = StanelyController(params)
        print(self.datasets)
        print(self.datasets["lat_lon_encoded"])
        self.original_lat_lon = polyline.decode(self.datasets["lat_lon_encoded"],precision=8)
        print(self.original_lat_lon)
        self.origin = self.original_lat_lon[0]
        self.origin = [self.origin[1], self.origin[0]]

        self.min_idx = -1
        tmp = []
        for p in self.original_lat_lon:
            enu = lla2enu(deg2rad(p[1]),deg2rad(p[0]),0,deg2rad(self.origin[0]),deg2rad(self.origin[1]),0)
            tmp.append([enu[0],enu[1]])
        self.xy = np.array(tmp).T
        theta = []
        if len(self.original_lat_lon) < 2:
            raise RuntimeError("path length error")
        elif len(self.original_lat_lon) == 2:
            dtheta = atan2(self.xy[1,1]-self.xy[1,0],self.xy[0,1]-self.xy[0,0])
            theta.append(dtheta)
            theta.append(dtheta)
        else:
            dtheta = np.arctan2(self.xy[1,2:]-self.xy[1,:-2],self.xy[0,2:]-self.xy[0,:-2])
            theta = [dtheta[0]] + dtheta.tolist() + [dtheta[-1]]
        theta = list(map(lambda x:x if x>=0 else x+2*math.pi,theta))
        self.xya = np.vstack((self.xy, np.array(theta)))
        
    def fore_check(self):
        return True

    def done_check(self):
        if len(self.original_lat_lon) < 31:
            return True
        return False

    def mission_action(self):
        cur_lat = self.publicResource.ugv_resource.get_gps_position()["latitude"]
        cur_lon = self.publicResource.ugv_resource.get_gps_position()["longitude"]
        self.enu = lla2enu(
            deg2rad(cur_lon),
            deg2rad(cur_lat),
            0,
            deg2rad(self.origin[0]),
            deg2rad(self.origin[1]),
            0,
        )
        yaw = self.publicResource.ugv_resource.get_gps_yaw()
        yaw = deg2rad(((360.0 - yaw) + 90) % 360.0)

        x = self.enu[0]
        y = self.enu[1]
        z = self.enu[2]
        
        if self.min_idx == -1:
            xd = self.xya[0, :]
            yd = self.xya[1, :]
            theta_d = self.xya[2, :]
        else:
            xd = self.xya[0, max(self.min_idx-self.BEFORE_NUM, 0):min(self.min_idx+self.AFTER_NUM, self.xya.shape[1])]
            yd = self.xya[1, max(self.min_idx-self.BEFORE_NUM, 0):min(self.min_idx+self.AFTER_NUM, self.xya.shape[1])]
            theta_d = self.xya[2, max(self.min_idx-self.BEFORE_NUM, 0):min(self.min_idx+self.AFTER_NUM, self.xya.shape[1])]
        idx = np.argmin((x - xd) ** 2 + (y - yd) ** 2)
        self.min_idx = idx + max(self.min_idx - self.BEFORE_NUM, 0)
        
        lat_err = -(x - xd[idx]) * np.sin(theta_d[idx]) + (y - yd[idx]) * np.cos(
            theta_d[idx]
        )
        yaw_err = yaw - theta_d[idx]
        if yaw_err > math.pi:
            yaw_err -= 2 * math.pi
        elif yaw_err < -math.pi:
            yaw_err += 2 * math.pi

        if not (-math.pi <= yaw_err <= math.pi):
            self.publicResource.ugv_resource.platform_interface.main_node.get_logger().warning("yaw_err calc error")

        self.publicResource.ugv_resource.platform_interface.main_node.get_logger().info(
            f"origin: {self.origin} local position: {self.enu=} error: {lat_err=} {yaw_err=}"
        )
        # print(f"{lat_err=}, {yaw_err=}")

        steer = self.controller.step(lat_err=lat_err, yaw_err=yaw_err)
        ###
        yaw_err *= 1
        alpha1 = 0.8
        alpha2 = 0.6
        tmp = 0.4*lat_err
        delta = alpha1*tmp + alpha2*yaw_err
        steer = math.atan(steer)
        print(f"{lat_err=:0.2f}, {yaw_err=:0.2f} steer={delta:0.2f}")
        speed = 0.5  # m/s
        self.publicResource.ugv_resource.set_vehicle_several_cmd(speed_mps=speed,steering_wheel_angle_deg=-delta*57.3,brk_en=0)
        # self.publicResource.ugv_resource.platform_interface.main_node.get_logger().info(
        #     f"control input: {steer=} {speed=}"
        # )

    def mission_enter(self):
        pass

    def mission_exit(self):
        pass

    def abort_check(self):
        pass
    
    def mission_abort(self):
        pass